/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2024-07-08     86188       the first version
 */
#ifndef APPLICATIONS_INC_MOTOR_CTR_H_
#define APPLICATIONS_INC_MOTOR_CTR_H_


#include "stm32f4xx_hal.h"


#define ENCONDE_HALL_VEL 11  ///霍尔编码器精度
#define TIME_PAS 4          ///AB双相组合得到4倍频

#define SPEED_RATIO 30    ///电机减速比为30

#define DT 10           ////定时器时间 10hz 100ms

#define RPM_VEL (ENCONDE_HALL_VEL*TIME_PAS*SPEED_RATIO) ///1320
#define PERIMETER 9.7f ///cm



enum {STOP,GOING,STOPPED,UDLOAD,RETURNING};

#define MOVE_FORWARD {LEFTFRONT_MOTOR_CW;RIGHTFRONT_MOTOR_CW;RIGHTBACK_MOTOR_CW;LEFTBACK_MOTOR_CW;}
#define MOVE_BACK    {LEFTFRONT_MOTOR_CCW;RIGHTFRONT_MOTOR_CCW;RIGHTBACK_MOTOR_CCW;LEFTBACK_MOTOR_CCW;}
#define MOVE_LEFT    {LEFTFRONT_MOTOR_CCW;RIGHTFRONT_MOTOR_CW;RIGHTBACK_MOTOR_CCW;LEFTBACK_MOTOR_CW;}
#define MOVE_RIGHT   {LEFTFRONT_MOTOR_CW;RIGHTFRONT_MOTOR_CCW;RIGHTBACK_MOTOR_CW;LEFTBACK_MOTOR_CCW;}

#define TURN_LEFT    {LEFTFRONT_MOTOR_CCW;RIGHTFRONT_MOTOR_CW;RIGHTBACK_MOTOR_CW;LEFTBACK_MOTOR_CCW;}
#define TURN_RIGHT   {LEFTFRONT_MOTOR_CW;RIGHTFRONT_MOTOR_CCW;RIGHTBACK_MOTOR_CCW;LEFTBACK_MOTOR_CW;}

#define CAR_STOP   {LEFTFRONT_MOTOR_STOP;RIGHTFRONT_MOTOR_STOP;RIGHTBACK_MOTOR_STOP;LEFTBACK_MOTOR_STOP;}


typedef struct{

    float speed;  // r/s
    float outVel;
    float outPwm;
    int32_t cnt;

    double pos;

}motor_t;

typedef struct{
    float x;
    float y;
    float z;

}attitude_t;

extern attitude_t desiredPos;
extern attitude_t actualPos;

extern attitude_t desiredVel;
extern attitude_t actualVel;


extern motor_t leftFrontMotor;
extern motor_t leftBackMotor;
extern motor_t rightFrontMotor;
extern motor_t rightBackMotor;

extern uint8_t car_sta;
extern uint8_t pidx_en;
extern uint8_t pidy_en;
extern uint8_t pidz_en;

void carSetTextPwm(uint16_t p);
void carSetTextPwm(uint16_t p);
void carStop(void);
void carSetPwm(void);
void motorPidInit(void);
void Kinematic_Analysis(void);

void transmitMotorCtor(uint16_t pwm,uint8_t dir);
void motorPidInterClear(void);
void posContorUpdata(void);

#endif /* APPLICATIONS_INC_MOTOR_CTR_H_ */
